//
// Created by xujingyi on 2021/5/16.
//
#include <Eigen/Dense>
#include <Serial.h>
#include "solve/solve_PNP.h"
#include "armor_finder.h"
#include "show_images.h"
#include "solve/Predictor.h"
#include "solve/SmoothKF.h"
#include "log.h"

extern cv::Mat show_pic,testboard;
extern rm::Predictor predictor;
extern rm::GyroPredictor gyro_pretr;
extern rm::Smoother smoother;
/*!
 * 用于以像素为单位进行枪口倾斜、子弹重力（、沿轨道运动）的补偿计算（相机距离转轴偏移在坐标系转换里加）
 * @param pic_center
 * @param armor
 * @param distance
 * @return cv::Point3f(pitch,yaw,distance)
 */
cv::Point3f rm::ArmorFinder::pixelAngleSolve(const cv::Point2f pic_center,rm::Armor &armor,const float distance){
    float y=pic_center.y+tan(rm_setter.pitch_angle_compensate/ANGLE_PER_RAD)*FOCUS_PIXAL*FOCUS;
    float x=pic_center.x+tan(rm_setter.yaw_angle_compensate/ANGLE_PER_RAD)*FOCUS_PIXAL*FOCUS;
    cv::Point2f center=cv::Point2f(x,y);
#if SHOOTER_MODE==DOWN_SHOOTER
    if(armor.box_pos.getState()==rm::GROUND) armor.traject_dwy=trajectory.getFireAngle(serial.rxpackage.bullet_spd,pixelToAngle(center).x,distance);//zhonglibuchang
    if(armor.box_pos.getState()==rm::HIGHLAND) armor.traject_dwy=trajectory.getFireAngle_highland(serial.rxpackage.bullet_spd,pixelToAngle(center).x,distance);
#else
    if(armor.box_pos.getState()==rm::HIGHLAND) {
        armor.traject_dwy=trajectory.getFireAngle_highland(serial.rxpackage.bullet_spd,pixelToAngle(center).x,distance);
    }
    if(armor.box_pos.getState()==rm::GROUND) armor.traject_dwy=trajectory.getFireAngle(serial.rxpackage.bullet_spd,pixelToAngle(center).x,abs(distance));
#endif
    center.y+=armor.traject_dwy;
//    drawCross(show_pic,center,COLOR_WHITE/5);
    cv::Point2f py= pixelToAngle(center);
    float pitch=py.x;
    float yaw=py.y;
//    armor.move_coeff_angle=trajectory.getMoveCoeffAngle(int(serial.rxpackage.chassis_speed.type_short),yaw,serial.rxpackage.yaw_angle.type_short/10.0f,distance);
//    yaw+=armor.move_coeff_angle;
//    center= angleToPixel(cv::Point2f(pitch,yaw));
//    drawCross(show_pic,center,COLOR_ORANGE/5);
    armor.box_pos.setPixelCenter(center,cv::Point3f(tan(yaw/ANGLE_PER_RAD)*distance,tan(pitch/ANGLE_PER_RAD)*distance,distance));
    return cv::Point3f(pitch,yaw,distance);
}

/*!
 * 姿态解算，包括：
 * 1.pnp测距
 * 2.相对角度及相对坐标结算
 * 3.世界坐标结算（以哨兵轨道最右端为原点，的敌人位置正方向：右(+x)，下(+y)，前(+z)）
 * @param armor 目标装甲板
 * @param offset ROI区域左上角坐标（偏移量）
 * @return cv::Point3f(pitch,yaw,distance)
 */
cv::Point3f rm::ArmorFinder::posResolve(Armor &armor) {
    systime t1,t2;
    getsystime(t1);

    cv::Point3f pnp_message,pixel_message,pos_message;//messgae=cv::Point3f(pitch,yaw,distance)

    if (rm_setter.show_armor&&armor.getID()!=-1) {
        cv::circle(show_pic,armor.center(),4,cv::Scalar(20, 20, 40),2);

        cv::Point3f pnp_xyz;
        pnp_xyz=armor.box_pos.getPNPCen3d();
        if (pnp_xyz.z != 0) {
            pnp_message.z = pnp_xyz.z;
            pnp_message.x =atan(pnp_xyz.y / pnp_xyz.z) *180.0f/CV_PI;
            pnp_message.y = atan(pnp_xyz.x / pnp_xyz.z) * 180.0f /CV_PI;
        }
        else {
            pnp_message.x = 0;
            pnp_message.y = 0;
            pnp_message.z = 0;
        }
//        LOGM("PNP_Z:%lf",pnp_message.z);
        cv::Point2f py= pixelToAngle(armor.box_pos.getPicCen2d());
//        std::cout<<armor.couple.getAbsYaw()<<std::endl;
        if(armor.couple.getBlindFlag()) pnp_xyz.z=3300.0f/armor.couple.getAbsYaw();
        pixel_message= cv::Point3f(py.x,py.y,pnp_xyz.z);
        armor.box_pos.setPixelCenter(armor.box_pos.getPicCen2d(),cv::Point3f(tan(py.y/ANGLE_PER_RAD)*pnp_xyz.z,tan(py.x/ANGLE_PER_RAD)*pnp_xyz.z,pnp_xyz.z));
        cv::circle(show_pic,armor.box_pos.getPicCen2d(),2,COLOR_RED,-1);
        armor.box_pos.setWorldCoord(worldCoordinateResolver(armor.box_pos.getPixelCen3d(),R_vec,serial.rxpackage.chassis_location.type_short));//must before pixelAngleSolve
        float bullet_spd=serial.rxpackage.bullet_spd<18?26:serial.rxpackage.bullet_spd;
        armor.delay_t=(armor.box_pos.getWorldCoord().z/(1000*bullet_spd))+rm_setter.delay_time_compensate;
#if SOLVE_MODE==PNP_MODE
        pos_message=pnp_message+(rm_setter.pitch_angle_compensate,rm_setter.yaw_angle_compensate,0);
        pos_message.x=pos_message.x+tan(trajectory.getFireAngle_highland(serial.rxpackage.bullet_spd,pos_message.x,distance)/ANGLE_PER_RAD)*FOCUS_PIXAL*FOCUS;
#elif SOLVE_MODE==PIXEL_ANGLE_MODE
        pos_message=pixelAngleSolve(armor.box_pos.getPicCen2d(),armor,pnp_xyz.z);
#endif
    }
    showArmor(show_pic, armor);//must after setworldcoord
    getsystime(t2);
    LOGT("posResolve:%.1fms",getTimeIntervalms(t2,t1));
    return pos_message;
}

/*!
 * 反卡尔曼预测（超调）函数
 * @param current 当前值
 * @param predict 当前预测值
 * @param lastPredict 上一时刻预测值
 * @param threshold 生效阈值
 * @param anti_range 补偿值 = anti_range*（当前时刻-上一时刻）
 * @param mode 0:在predict的基础上加 1：在current基础上加
 * @return
 */
float antiPredict(float current,float predict,float lastPredict,float threshold,float anti_range,int mode){
    int anti_kalman;
    if(mode==0){
        if((predict + anti_range*(current - lastPredict))<=PICTURE_COL_NUM && (predict + anti_range*(current - lastPredict))>=0){//Prevent Anti-kal out of Mat
            if(abs(current - lastPredict) > threshold) anti_kalman = predict + anti_range*(current - lastPredict);
            else anti_kalman = predict;
        }
        else anti_kalman = predict;
        return anti_kalman;
    }
    if(mode==1){
        if((current + anti_range*(current - predict))<=PICTURE_COL_NUM && (current + anti_range*(current - predict))>=0){//Prevent Anti-kal out of Mat
            if(abs(current - predict) > threshold) anti_kalman = current + anti_range*(current - predict);
            else anti_kalman = current;
        }
        else anti_kalman = current;
        return anti_kalman;
    }
}

cv::Point2f antiPredict(cv::Point2f currentPoint,cv::Point2f prePoint,cv::Point2f lastPrePoint){
    float anti_range=rm_setter.anti_kalman_compensate_coefficient;
    cv::Point2f anti_kalmanPoint;

    anti_kalmanPoint.x=antiPredict(currentPoint.x,prePoint.x,lastPrePoint.x,8,anti_range,0);
    anti_kalmanPoint.y=antiPredict(currentPoint.y,prePoint.y,lastPrePoint.y,3,anti_range,0);

//    std::cout<<"antiPredict:now"<<currentPoint<<" pre:"<<prePoint<<" lastpre:"<<lastPrePoint<<" anti:"<<anti_kalmanPoint<<std::endl;
    return anti_kalmanPoint;
}

/*!
 * 非陀螺阶段预测
 * @param armor 目标装甲
 * @param offset ROI区域偏置补偿，一般为_track_box.tl()
 * @return pos_message:cv::Point3f(pitch,yaw,distance)
 */
cv::Point3f rm::ArmorFinder::pre4Normal(){
    if (target_armor.getID()>0) posResolve(target_armor);
    else return cv::Point3f(0,0,0);
//#if SHOOTER_MODE==DOWN_SHOOTER
//    //    target_armor.box_pos.setWorldCoord(smoother.KFc->renew(target_armor.box_pos.getWorldCoord()));
//#else
////    target_armor.box_pos.setWorldCoord(smoother.KFc->renew(target_armor.box_pos.getWorldCoord()));
//#endif
    cv::Point3f pixel_message,pos_message;//messgae=cv::Point3f(pitch,yaw,distance)
    systime t1,t2;
    getsystime(t1);

    if(target_armor.getID()!=last_armor.getID()){
        track_state=rm::CHANGE_ENEMY;
        predictor.Boxposes.clear();
        smoother.init();
    }
    else if((abs(target_armor.couple.getBoxAngle()-last_armor.couple.getBoxAngle())>rm_setter.change_armor_angle)||(abs(target_armor.center().x-last_armor.center().x)>50)) {
        track_state=rm::CHANGE_ARMOR;
    }
    else track_state=rm::TRACK_ARMOR;

    if(predictor.Boxposes.size()<predictor.posSize) {
        predictor.Boxposes.emplace_back(target_armor.box_pos);
    }
    else{
        for(int fi = 1;fi<predictor.posSize;++fi){
            predictor.Boxposes[fi-1]= predictor.Boxposes[fi];
        }
        predictor.Boxposes[predictor.posSize-1]=target_armor.box_pos;
    }

    float equalize_dis=0;
    for (rm::BoxPos pos:predictor.Boxposes) equalize_dis+=pos.getPixelCen3d().z;
    equalize_dis=0.70*equalize_dis/predictor.Boxposes.size()+0.30*predictor.Boxposes.back().getPixelCen3d().z;

#if PRE_MODE==PIXEL_MODE
    //    std::cout<<armor.box_pos.getPicCen2d()<<std::endl;
    pixel_message=pixelAngleSolve(target_armor.box_pos.getPicCen2d(),target_armor,equalize_dis);
    float dy=tan((rm_setter.pitch_angle_compensate)/ANGLE_PER_RAD)*FOCUS_PIXAL*FOCUS+target_armor.traject_dwy;
    float dx=tan(rm_setter.yaw_angle_compensate/ANGLE_PER_RAD)*FOCUS_PIXAL*FOCUS;
    cv::Point2f pre_cen=predictor(target_armor.box_pos.getPicCen2d(),equalize_dis,target_armor.delay_t);
#if SOLVE_MODE==ANTI_PREDICT_MODE
    cv::Point2f anti_center=antiPredict(target_armor.box_pos.getPicCen2d(),pre_cen,predictor.last_no_cmpns_pre_cen);
    predictor.last_no_cmpns_pre_cen=pre_cen;/*一定要在antiPredict之后*/
    cv::circle(show_pic,pre_cen-cv::Point2f(dx,dy),4,COLOR_GREEN/5,2);
    pre_cen=anti_center;
#endif
#elif PRE_MODE==COORD_MODE
//    std::cout<<gyro_tool.qOmegas[-1]*gyro_tool.qOmegas[-1]/1000<<std::endl;
    cv::Point2f pre_cen=predictor(track_state,chassis_state,target_armor.box_pos.getWorldCoord(),target_armor.delay_t);

    cv::Point2f delta_cen=smoother.KFPixel->renew(pre_cen-target_armor.center());
    pre_cen=target_armor.center()+delta_cen;

    pixel_message=pixelAngleSolve(target_armor.box_pos.getPicCen2d(),target_armor,equalize_dis);
    float dy=tan((rm_setter.pitch_angle_compensate)/ANGLE_PER_RAD)*FOCUS_PIXAL*FOCUS+target_armor.traject_dwy;
    float dx=tan(rm_setter.yaw_angle_compensate/ANGLE_PER_RAD)*FOCUS_PIXAL*FOCUS;
    cv::Point2f cmpns=cv::Point2f(dx,dy);
    pre_cen+=cmpns;
#if SOLVE_MODE==ANTI_PREDICT_MODE
    cv::Point2f anti_center=antiPredict(target_armor.center(),pre_cen-cv::Point2f(dx,dy),predictor.last_no_cmpns_pre_cen);
    predictor.last_no_cmpns_pre_cen=pre_cen-cv::Point2f(dx,dy);/*一定要在antiPredict之后*/
    cv::circle(show_pic,pre_cen-cv::Point2f(dx,dy),4,COLOR_GREEN/5,2);
    pre_cen=anti_center+cv::Point2f(dx,dy);
#endif
#endif

    if(predictor.Boxposes.size()<predictor.posSize){
        pos_message=pixel_message;
        cv::circle(show_pic,target_armor.box_pos.getPicCen2d(),4,cv::Scalar(24, 51, 51),2);
        cv::circle(show_pic,target_armor.box_pos.getPicCen2d()-cv::Point2f(dx,dy),4,cv::Scalar(24, 51, 51),2);//差值为预测量
    }
    else {
        cv::Point3f predict_pixel_message;
        cv::Point2f pyangle= pixelToAngle(pre_cen);
//        predict_pixel_message=cv::Point3f(pyangle.x, pyangle.y, 4000);
        predict_pixel_message=cv::Point3f(pyangle.x, pyangle.y, equalize_dis);
        pos_message=predict_pixel_message;
        drawCross(show_pic,pre_cen,COLOR_LIGHTYELLOW/5);
        cv::circle(show_pic,pre_cen-cv::Point2f(dx,dy),4,COLOR_LIGHTYELLOW/5,2);
    }

//        LOGM("pnp:(%lf,%lf) pixel:(%lf,%lf)",target_armor.box_pos.getPNPCen3d().x,target_armor.box_pos.getPNPCen3d().y,target_armor.box_pos.getPixelCen3d().x,target_armor.box_pos.getPixelCen3d().y);
//        LOGM("pnp:(%lf,%lf) pixel:(%lf,%lf)",pnp_message.x,pnp_message.y,pixel_message.x,pixel_message.y);

//        target_armor.move_coeff_angle=trajectory.getMoveCoeffAngle(int(serial.rxpackage.chassis_speed.type_short),pos_message.y,serial.qYaws[-1],pos_message.z);
//        pos_message.y+=armor.move_coeff_angle;
//        cv::Point2f center= angleToPixel(cv::Point2f(pos_message.x,pos_message.y));
//        drawCross(show_pic,center,COLOR_ORANGE/5);
    char c_angle[100],c_pre[100],c_absyaw[100];
    sprintf(c_angle, "angle=%.1f", target_armor.couple.getBoxAngle());
    putText(show_pic, c_angle, cv::Point(target_armor.center().x - 150, target_armor.center().y -90), cv::FONT_HERSHEY_TRIPLEX, 0.8,COLOR_WHITE);
    sprintf(c_pre, "pre_state=%d", int(track_state));
    putText(show_pic, c_pre, cv::Point(target_armor.center().x - 150, target_armor.center().y -60), cv::FONT_HERSHEY_TRIPLEX, 0.8,COLOR_WHITE);
    sprintf(c_absyaw, "abs_ang=%.0f", serial.qYaws[-1]+target_armor.couple.getEular()[EULAR_AXIS]);
    putText(show_pic, c_absyaw, cv::Point(target_armor.center().x - 150, target_armor.center().y -120), cv::FONT_HERSHEY_TRIPLEX, 0.8,COLOR_WHITE);
    getsystime(t2);
    LOGT("pre4Normal:%.1fms",getTimeIntervalms(t2,t1));
    return pos_message;
}

extern int last_fps;
/*!
 * 陀螺阶段预测
 * @param gyro_armors 为同一陀螺上的相同数字装甲
 * @param offset ROI区域偏置补偿，一般为_track_box.tl()
 * @return pos_message:cv::Point3f(pitch,yaw,distance)
 */
//cv::Point3f rm::ArmorFinder::pre4Gyro(rm::Armors& gyro_armors,cv::Point2f offset){
//    if(gyro_armors.empty()) return cv::Point3f(0,0,0);
//    for(rm::Armor &armor : gyro_armors) {
//        armor.setOffset(offset);
//        posResolve(armor,offset);
//    }
//    if (gyro_armors.size() > 1) {
//        if(gyro_armors[0].center().x<gyro_armors[1].center().x){
//            gyro_tool.left_armor=gyro_armors[0];
//            gyro_tool.right_armor=gyro_armors[1];
//        }
//        else{
//            gyro_tool.left_armor=gyro_armors[1];
//            gyro_tool.right_armor=gyro_armors[0];
//        }
//    }
//    else{
//        gyro_tool.left_armor=gyro_armors[0];
//        gyro_tool.right_armor=gyro_armors[0];
//    }
//    rm::Armor tmparmor;
//    if(gyro_tool.getState()==rm::GyroTool::UNKNOWN||gyro_tool.getState()==rm::GyroTool::LEVO){
//        tmparmor=gyro_tool.right_armor;
//    }
//    else {
//        tmparmor=gyro_tool.left_armor;
//    }
//    if(_spinning_state_flag==true) target_armor=tmparmor;
//
//    gyro_tool.omega = gyro_pretr.v*last_fps;
//    gyro_tool.qEulars.push(tmparmor.box_pos.getrodetoEularVec()[1]);
//    gyro_tool.qOmegas.push(gyro_tool.omega);
//    gyro_tool.qWorldCoords.push(tmparmor.box_pos.getWorldCoord());
//    if(gyro_tool.omega>20)
//        gyro_tool.setState(rm::GyroTool::LEVO);
//    else if(gyro_tool.omega<-20)
//        gyro_tool.setState(rm::GyroTool::DEXTRO);
//    else gyro_tool.setState(rm::GyroTool::UNKNOWN);
//
//    int cnt=0;
//    gyro_tool.gyro_center=cv::Point3f(0,0,0);
//    for(int i=0;i<gyro_tool.qWorldCoords.length();i++){
//    if(cos(gyro_tool.qEulars[i]/ANGLE_PER_RAD)>0.996||cos(gyro_tool.qEulars[i]/ANGLE_PER_RAD)<0.01){
//        cnt++;
//        cv::Point3f world_coor=gyro_tool.qWorldCoords[i];
//        gyro_tool.gyro_center.x+=world_coor.x;
//        gyro_tool.gyro_center.y+=world_coor.y;
//        gyro_tool.gyro_center.z+=world_coor.z;
////        if(abs(gyro_tool.gyro_center.y-world_coor.y)>56)
////            gyro_tool.gyro_center.y = (gyro_tool.gyro_center.y+world_coor.y)/2;
//    }
//    }
//    gyro_tool.gyro_center.x=0.8*gyro_tool.gyro_center.x/(float)cnt+0.2*gyro_tool.qWorldCoords[-1].x;
//    gyro_tool.gyro_center.z=0.8*gyro_tool.gyro_center.z/(float)cnt+0.2*gyro_tool.qWorldCoords[-1].z;
//    gyro_tool.gyro_center.y=0.8*gyro_tool.gyro_center.y/(float)cnt+0.2*gyro_tool.qWorldCoords[-1].y;
//
//    cv::Point2f gyro_pixel= angleToPixel(referCoordinate2Angle(antiWorldCoordinateResolver(gyro_tool.qWorldCoords[-1],R_back,(float)serial.rxpackage.chassis_location.type_short)));
//
//    //    cv::Point2f gyro_pixel= angleToPixel(referCoordinate2Angle(antiWorldCoordinateResolver(tmparmor.box_pos.getWorldCoord(),R_back,(float)serial.rxpackage.chassis_location.type_short)));
//    cv::Point2f pre_xNangle = gyro_pretr.predict(gyro_pixel.x,tmparmor.box_pos.getrodetoEularVec()[1],1,tmparmor.delay_t);
//
//    cv::Point3f gyro_message=pixelAngleSolve(gyro_pixel,tmparmor,tmparmor.box_pos.getPixelCen3d().z);
//#ifdef GYRO_MODE
//#if GYRO_MODE==CENTER_MODE
//    if(_spinning_state_flag){
//    cv::circle(show_pic,gyro_pixel,4,COLOR_LIGHTYELLOW/5,2);
//    drawCross(show_pic, angleToPixel(cv::Point2f(gyro_message.x,gyro_message.y)),COLOR_LIGHTYELLOW/5);
//    }
//    gyro_tool.setPosMessage(gyro_message);
//#endif
//#endif
////    float dy=tan((rm_setter.pitch_angle_compensate)/ANGLE_PER_RAD)*FOCUS_PIXAL*FOCUS+tmparmor.traject_dwy;
////    float dx=tan(rm_setter.yaw_angle_compensate/ANGLE_PER_RAD)*FOCUS_PIXAL*FOCUS;
////    pre_cen+=cv::Point2f(dx,dy);
//    return gyro_tool.getPosMessage();
//}//陀螺预测

cv::Point3f rm::ArmorFinder::pre4Gyro(rm::Armors& gyro_armors,cv::Point2f offset){
    for(rm::Armor &armor : gyro_armors) {
        posResolve(armor);
    }
    if (gyro_armors.size() > 1) {
        if(gyro_armors[0].center().x<gyro_armors[1].center().x){
            gyro_tool.left_armor=gyro_armors[0];
            gyro_tool.right_armor=gyro_armors[1];
        }
        else{
            gyro_tool.left_armor=gyro_armors[1];
            gyro_tool.right_armor=gyro_armors[0];
        }
    }
    else{
        gyro_tool.left_armor=gyro_armors[0];
        gyro_tool.right_armor=gyro_armors[0];
    }
    rm::Armor tmparmor;
    if(gyro_tool.getState()==rm::GyroTool::UNKNOWN||gyro_tool.getState()==rm::GyroTool::LEVO){
        tmparmor=gyro_tool.right_armor;
    }
    else {
        tmparmor=gyro_tool.left_armor;
    }
    if(_spinning_state_flag==true) target_armor=tmparmor;

    gyro_tool.omega = gyro_pretr.v*last_fps;
    gyro_tool.qEulars.push(serial.qYaws[-1]-tmparmor.box_pos.getrodetoEularVec()[EULAR_AXIS]);
    char cang[100];
    sprintf(cang,"qYaws[-1]:%.1lf eular:%lf",serial.qYaws[-1],tmparmor.box_pos.getrodetoEularVec()[EULAR_AXIS]);
    cv::putText(show_pic,cang,cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y),1,3,COLOR_WHITE,2);
//    std::cout<<serial.qYaws[-1]-gyro_tool.qEulars[-1]<<std::endl;
    gyro_tool.qOmegas.push(gyro_tool.omega);
    gyro_tool.qWorldCoords.push(tmparmor.box_pos.getWorldCoord());
    if(gyro_tool.omega>20)
        gyro_tool.setState(rm::GyroTool::LEVO);
    else if(gyro_tool.omega<-20)
        gyro_tool.setState(rm::GyroTool::DEXTRO);
    else gyro_tool.setState(rm::GyroTool::UNKNOWN);

    int cnt=0;
    gyro_tool.gyro_center=cv::Point3f(0,0,0);

//    int max_width=((gyro_tool.getID()%7)==1)?400:300;
    int max_width=400;
//    std::cout<<max_width<<std::endl;
    for(int i=0;i<gyro_tool.qWorldCoords.length();i++){
        if(cos((serial.qYaws[-1]-gyro_tool.qEulars[i])/ANGLE_PER_RAD)>0.950){
            cnt++;
            cv::Point3f world_coor=gyro_tool.qWorldCoords[i];
            gyro_tool.gyro_center.x+=world_coor.x+max_width*sin(gyro_tool.qEulars[-1]/ANGLE_PER_RAD);
            gyro_tool.gyro_center.y+=world_coor.y;
            gyro_tool.gyro_center.z+=world_coor.z+max_width*cos(gyro_tool.qEulars[-1]/ANGLE_PER_RAD);
//        if(abs(gyro_tool.gyro_center.y-world_coor.y)>56)
//            gyro_tool.gyro_center.y = (gyro_tool.gyro_center.y+world_coor.y)/2;
        }
    }
            gyro_tool.gyro_center/=(float)cnt;
//std::cout<<gyro_tool.gyro_center<<std::endl;
    cv::Point3f refer_gyro_center=antiWorldCoordinateResolver(gyro_tool.gyro_center,R_back,(float)serial.rxpackage.chassis_location.type_short);
    cv::Point2f gyro_center_pixel= angleToPixel(referCoordinate2Angle(refer_gyro_center));
    cv::Point2f gyro_pixel= angleToPixel(referCoordinate2Angle(refer_gyro_center-cv::Point3f(0,0,max_width)));
    //    cv::Point2f gyro_pixel= angleToPixel(referCoordinate2Angle(antiWorldCoordinateResolver(tmparmor.box_pos.getWorldCoord(),R_back,(float)serial.rxpackage.chassis_location.type_short)));
    cv::Point2f pre_xNangle = gyro_pretr.predict(gyro_pixel.x,tmparmor.box_pos.getrodetoEularVec()[EULAR_AXIS],1,tmparmor.delay_t);

    cv::Point3f gyro_message;
    if(gyro_tool.qWorldCoords.length()>0) gyro_message=pixelAngleSolve(gyro_pixel,tmparmor,tmparmor.box_pos.getPixelCen3d().z);
    else gyro_message=pixelAngleSolve(tmparmor.box_pos.getPicCen2d(),tmparmor,tmparmor.box_pos.getPixelCen3d().z);
#ifdef GYRO_MODE
#if GYRO_MODE==CENTER_MODE
    if(_spinning_state_flag){
        cv::circle(show_pic,gyro_center_pixel,4,COLOR_CHOCOLATE/5,2);
        cv::circle(show_pic,gyro_pixel,4,COLOR_LIGHTYELLOW/5,2);
        drawCross(show_pic, angleToPixel(cv::Point2f(gyro_message.x,gyro_message.y)),COLOR_LIGHTYELLOW/5);
        showGryo(show_pic,gyro_tool,_track_box.br(),COLOR_CHOCOLATE/5);
    }
    cv::circle(show_pic,gyro_center_pixel,6,COLOR_GREEN/5,3);
    gyro_tool.setPosMessage(gyro_message);
#endif
#endif
//    float dy=tan((rm_setter.pitch_angle_compensate)/ANGLE_PER_RAD)*FOCUS_PIXAL*FOCUS+tmparmor.traject_dwy;
//    float dx=tan(rm_setter.yaw_angle_compensate/ANGLE_PER_RAD)*FOCUS_PIXAL*FOCUS;
//    pre_cen+=cv::Point2f(dx,dy);
    return gyro_tool.getPosMessage();
}//陀螺预测

/*!
 *  旋转矩阵R_vec、逆矩阵(=转置矩阵)R_back的成员变量计算
 */
void rm::ArmorFinder::_calcRotMatrix(){
    float alpha=-serial.qPitches[-1];//up is zheng
    float beta=-serial.qYaws[-1];//right is zheng
    float gamma=0;
    Eigen::Matrix<float,3,3> x_Rvec,y_Rvec,z_Rvec;
    float sin_a=sin(alpha/ANGLE_PER_RAD),cos_a=cos(alpha/ANGLE_PER_RAD),sin_b=sin(beta/ANGLE_PER_RAD),cos_b=cos(beta/ANGLE_PER_RAD),sin_g=sin(gamma/ANGLE_PER_RAD),cos_g=cos(gamma/ANGLE_PER_RAD);
//    std::cout<<"alpha:"<<alpha<<" beta:"<<beta<<std::endl;
//    std::cout<<"sin(a):"<<sin_a<<" cos(a):"<<cos_a<<std::endl;
//    std::cout<<"sin(b):"<<sin_b<<" cos(b):"<<cos_b<<std::endl;
    z_Rvec<<
          cos_g,          -sin_g,                  0,
            sin_g,           cos_g,                  0,
            0,               0,                  1;
    x_Rvec<<
          1,               0,                  0,
            0,           cos_a,              -sin_a,
            0,           sin_a,              cos_a;
    y_Rvec<<
          cos_b,               0,              sin_b,
            0,               1,                  0,
            -sin_b,               0,              cos_b;
    R_vec=z_Rvec*y_Rvec*x_Rvec;
    R_back=R_vec.transpose();
}





